#include "CSeaAmp.h"

CML_NAMESPACE_USE();



CSeaAmp::CSeaAmp( CanOpen &co, int16 nodeID, int tp )
{
	int i;
	Amp(co,nodeID);
	timerPeriod = tp;
	for(i=0;i<CACTION_HASH_SIZE;i++){
		hash[i]=NULL;
	}
}

CSeaAmp::CSeaAmp( CanOpen &co, int16 nodeID, AmpSettings &settings, int tp )
{
	int i;
	Amp(co, nodeID,settings);
	timerPeriod = tp;
	for(i=0;i<CACTION_HASH_SIZE;i++){
		hash[i]=NULL;
	}
}

const Error *CSeaAmp::Init( CanOpen &co, int16 nodeID, int tp )
{
	timerPeriod = tp;
	InitTimer();
	return Amp::Init(co,nodeID);
}

const Error *CSeaAmp::Init( CanOpen &co, int16 nodeID, AmpSettings &settings, int tp )
{
	timerPeriod = tp;
	InitTimer();
	return Amp::Init(co, nodeID,settings);
}


CSeaAmp::~CSeaAmp(void)
{
}

void CSeaAmp::InitTimer()
{
	timer = new QTimer(this);
	//connect(timer, SIGNAL(timeout()),this, SLOT(TimerOutEvent()));

	return;
}

void CSeaAmp::TimerOutEvent()
{
	int i;

	CAction **pp;

	seaHashMutex.Lock();
	dataMutex.Lock();

	for(i=0;i<CACTION_HASH_SIZE;i++)
	{
		pp = &hash[i];
		while(*pp)
		{
			(*pp)->DoAct();
			pp = &( (*pp)->next );
		}
	}

	seaHashMutex.Unlock();
	dataMutex.Unlock();


	return ;
}


bool CSeaAmp::EnableAction( class CAction *aPtr )
{
	bool err = 0;

	seaHashMutex.Lock();

	CAction **pp = searchHash( aPtr->id );

	if( *pp )
		err = 1;
	else
	{
      aPtr->next = 0;
      *pp = aPtr;
	}

	seaHashMutex.Unlock();

	return err;

}
	
bool CSeaAmp::DisableAction( class CAction *aPtr )
{
	bool err = 0;

	seaHashMutex.Lock();

	CAction **pp = searchHash( aPtr->id );

	if( !*pp )
		err = 1;

	else
	{
		aPtr = *pp;
		*pp = aPtr->next;

		// Point the receiver back at itself.
		// This allows the receiver to determine it's disabled.
		aPtr->next = aPtr;
	}

	seaHashMutex.Unlock();
	return err;
}

CAction **CSeaAmp::searchHash( int id )
{
   int i = id % CACTION_HASH_SIZE;

   CAction **pp = &hash[i];

   while( 1 )
   {
      if( !*pp )
	 return pp;

      int x = (*pp)->id;

      if( id == x )
	 return pp;

      pp = &( (*pp)->next);
   }
}

void CSeaAmp::SetPosMotor( double pm )
{	
	dataMutex.Lock();
	if(pm<PI)
		posMotor = pm; 
	else if(pm>PI)
		posMotor = pm - 2*PI;
	posSpring = posLoad - posMotor - posSpringZero;
	if( posSpring<(-PI/2))
		posSpring = posSpring + 2*PI;
	else if( posSpring>(PI/2))
		posSpring = posSpring - 2*PI;
#ifdef DEATHAREA
	if(posSpring>DEADHIGH) posSpring = posSpring-DEADHIGH;
	else if(posSpring<DEADLOW) posSpring = posSpring-DEADLOW;
	else  posSpring = 0;
#endif

	dataMutex.Unlock();
}

void CSeaAmp::SetVelMotor(double vm)
{
	dataMutex.Lock();
	velMotor = vm;
	if(posSpring<DEADHIGH&&posSpring>DEADLOW)
		velSpring=0;
	else velSpring = velLoad - velMotor;
	dataMutex.Unlock();
}

void CSeaAmp::SetAccMotor(double am)
{
	dataMutex.Lock();
	accMotor = am;
	if(posSpring<DEADHIGH&&posSpring>DEADLOW)
		accSpring=0;
	else accSpring = accLoad - accMotor;
	dataMutex.Unlock();
}

void CSeaAmp::SetPosLoad( double pl )
{	
	dataMutex.Lock();
	if(pl<PI)
		posLoad = pl; 
	else if(pl>PI)
		posLoad = pl - 2*PI; 
	posSpring = posLoad - posMotor - posSpringZero;
	if( posSpring<(-PI/2))
		posSpring = posSpring + 2*PI;
	else if( posSpring>(PI/2))
		posSpring = posSpring - 2*PI;
#ifdef DEATHAREA
	if(posSpring>DEADHIGH) posSpring = posSpring-DEADHIGH;
	else if(posSpring<DEADLOW) posSpring = posSpring-DEADLOW;
	else  posSpring = 0;
#endif
	dataMutex.Unlock();
}

void CSeaAmp::SetVelLoad(double vl)
{
	dataMutex.Lock();
	velLoad = vl;
	if(posSpring<DEADHIGH&&posSpring>DEADLOW)
		velSpring=0;
	else velSpring = velLoad - velMotor;
	dataMutex.Unlock();
}

void CSeaAmp::SetAccLoad(double al)
{
	dataMutex.Lock();
	accLoad = al;
	if(posSpring<DEADHIGH&&posSpring>DEADLOW)
		accSpring=0;
	else accSpring = accLoad - accMotor;
	dataMutex.Unlock();
}

void CSeaAmp::SetForceSet(double fs)
{
	dataMutex.Lock();
	forceSet = fs;
	dataMutex.Unlock();
}

void CSeaAmp::SetVelForceSet(double fvs)
{
	dataMutex.Lock();
	velForceSet = fvs;
	dataMutex.Unlock();
}

void CSeaAmp::SetAccForceSet(double accforceset)
{
	dataMutex.Lock();
	AccForceSet=accforceset;
	dataMutex.Unlock();
}

void CSeaAmp::SetVelPositionSet(double vps)
{
	dataMutex.Lock();
	velPositionSet = vps;
	dataMutex.Unlock();
}

void CSeaAmp::SetPositionSet(double ps)
{
	dataMutex.Lock();
	positionSet = ps;
	dataMutex.Unlock();
}

void CSeaAmp::SetVelVelocitySet(double vvs)
{
	dataMutex.Lock();
	velVelocitySet = vvs;
	dataMutex.Unlock();
}

void CSeaAmp::SetVelocitySet(double vs)
{
	dataMutex.Lock();
	velocitySet = vs;
	dataMutex.Unlock();
}
void CSeaAmp::SetAccPositionSet( double vps )
{
	dataMutex.Lock();
	AccPositionSet= vps;
	dataMutex.Unlock();

}





CAction::CAction( void )
{
	next = this;
	sa = 0;
}

CAction::CAction( CSeaAmp &seaAmp, int id )
{
	next = this;
	sa = 0;
	Init( seaAmp, id );
}

CAction::~CAction( void )
{
	UnInit();
}

bool CAction::Init( CSeaAmp &seaAmp, int id )
{
	DisableAction();

	this->id = id;
	sa = &seaAmp;
	return 0;
}

bool CAction::UnInit()
{
	DisableAction();
	sa = 0;
	return 0;
}

void CAction::DoAct( void )
{
	return;
}

int CAction::getActionID( void )
{
	return id;
}

bool CAction::EnableAction( void )
{
	if( !sa ) return 1;
	
	return sa->EnableAction( this );
}

bool CAction::DisableAction( void )
{
   if( !sa ) return 1;

   if( next == this ) return 1;

   return sa->DisableAction( this );
}